﻿using System;
using System.Collections.Generic;
using System.Text;

namespace LocalizationTest
{
  class ExperimentPose2DBearings : Experiment
  {
    public ExperimentPose2DBearings()
    {
      Globals.Init(this);

      map = new MapWithLandmarks(MapWithLandmarks.MapType.roboCupBeacons);
      motionModel = new MotionModelPose2D();
      sensors = new List<Sensor>();
      sensors.Add(new BearingSensor(MapWithLandmarks.LandmarkIDs.centerLeftFlag));
      sensors.Add(new BearingSensor(MapWithLandmarks.LandmarkIDs.centerRightFlag));
      sensors.Add(new BearingSensor(MapWithLandmarks.LandmarkIDs.opponentLeftGoalPost));
      sensors.Add(new BearingSensor(MapWithLandmarks.LandmarkIDs.opponentRightGoalPost));
      sensors.Add(new BearingSensor(MapWithLandmarks.LandmarkIDs.ownLeftGoalPost));
      sensors.Add(new BearingSensor(MapWithLandmarks.LandmarkIDs.ownRightGoalPost));

      sensors.Add(new BearingSensorAmbiguous(MapWithLandmarks.LandMarkClasses.corner));

      /*
      sensors.Add(new DistanceSensor(MapWithLandmarks.LandmarkIDs.centerLeftFlag));
      sensors.Add(new DistanceSensor(MapWithLandmarks.LandmarkIDs.centerRightFlag));
      sensors.Add(new DistanceSensor(MapWithLandmarks.LandmarkIDs.opponentLeftGoalPost));
      sensors.Add(new DistanceSensor(MapWithLandmarks.LandmarkIDs.opponentRightGoalPost));
      sensors.Add(new DistanceSensor(MapWithLandmarks.LandmarkIDs.ownLeftGoalPost));
      sensors.Add(new DistanceSensor(MapWithLandmarks.LandmarkIDs.ownRightGoalPost));
      */
      typeOfActionDescription = typeof(ContinuousActionPose2D);

      actionAndObservationHistory = new ActionAndObservationHistory();
      simulatedWorldState = new WorldStatePose2D(
        Globals.stateSpaceInitialX, 
        Globals.stateSpaceInitialY, 
        Globals.stateSpaceInitialRotation);
    }
  }
}
